#!/usr/bin/env python3
# cython: language_level=3

import random
import numpy as np
import time
import math
import matplotlib.pyplot as plt
from scan_icp import get_ob, icp, match_score, get_trans_matrix, get_rot_matrix, final_score

import rospy
import copy
import sys
sys.path.append("/home/ubuntu/wingbow_ws/src/wingbow_nav/script")
sys.path.append("/home/ubuntu/wingbow_ws/src/wing_scan/script")

import LaserFilter
from LaserFilter import get_obstacle_array
from ScanInfo import ScanInfo
from tf_py2 import euler_from_quaternion
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from threading import Thread

class Plot:
    @staticmethod
    def reset():
        plt.cla()
        plt.axis("equal")
        plt.grid(True)

    @staticmethod
    def plot_data(m, color="ob"):
        plt.plot(m[0, :], m[1, :], color, markersize=2)

'''
简易的搜索匹配点云方法
'''

class Core:
    def __init__(self):
        self.alive = True
        self.back_sub = None
        self.odom_sub = None
        self.back_info = ScanInfo('back')
        self.back_ob = None
        self.ob_update = False

    def init_subs(self):
        self.back_sub = rospy.Subscriber('scan_back', LaserScan, self.back_callback, queue_size=1)

    def init_scan_info(self, scan, scan_info, trans_r, trans_th, min_ang=-1.658062789, max_ang=1.658062789, threshold=100, max_range=1.0):
        scan_info.scan['ranges'] = list(scan.ranges)
        scan_info.scan['angle_min'] = scan.angle_min
        scan_info.scan['angle_increment'] = scan.angle_increment
        scan_info.scan['intensities'] = list(scan.intensities)
        LaserFilter.init_points(scan_info.scan, threshold=threshold, max_range=max_range, min_angle=min_ang, max_angle=max_ang)
        scan_info.scan = LaserFilter.transform(scan_info.scan, trans_r, trans_th)
        scan_info.DataReady = True

    def back_callback(self, scan):
        def reduce_points(scan, max_amount):
            size = len(scan.intensities)
            intesities = list(scan.intensities)
            if size > max_amount:
                intesities.sort()
                return intesities[size-max_amount]
            else:
                return None

        thre = reduce_points(scan, max_amount=200)
        if not thre is None:
            if thre < 80: thre = 80
            pass
        else:
            thre = 80
            
        self.init_scan_info(scan, self.back_info, 0.908, math.pi, min_ang=-math.pi/3, max_ang=math.pi/3, threshold=thre, max_range=8.0)

        self.back_ob = get_obstacle_array(self.back_info.scan).T
        np.save("laser_cache.npy", self.back_ob)
        Plot.reset()
        Plot.plot_data(self.back_ob)
        plt.pause(0.001)

    def run(self):
        try:
            rospy.init_node('laser_record', anonymous=False)
            rospy.loginfo('laser_record node start.')
            self.init_subs()
            rospy.spin()
        except BaseException as e:
            rospy.logwarn(e)
        finally:
            self.alive = False

node = Core()

if __name__ == "__main__":
    node.run()

